#include <functional>
#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "test_interfaces/action/fibonacci.hpp"
#include "visibility_control.h"

class FibonacciActionServer : public rclcpp::Node
{
public:
    using Fibonacci = test_interfaces::action::Fibonacci;                   // 该类型包含目标和反馈消息
    using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>; // 用于管理动作的类类型

    // 构造函数，fibonacci_action_server是节点名，NodeOptions用于指定节点的创建选项
    explicit FibonacciActionServer(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("fibonacci_action_server", options)
    {
        using namespace std::placeholders; // 使用std::placeholders 命名空间简化代码

        // 创建fibonacci动作服务器，使用create_server函数
        this->action_server_ = rclcpp_action::create_server<Fibonacci>(
            this,
            "fibonacci",
            std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),  // 处理目标
            std::bind(&FibonacciActionServer::handle_cancel, this, _1),    // 处理取消请求
            std::bind(&FibonacciActionServer::handle_accepted, this, _1)); // 处理已接受的目标
    }

private:
    rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

    /**
     * @brief 处理目标
     *
     * 当收到新目标请求时调用
     *
     * @param uuid 唯一标识符
     * @param goal 请求的目标值
     * @return rclcpp_action::GoalResponse
     */
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const Fibonacci::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
        (void)uuid;
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; // 表示接受目标并执行
    }

    /**
     * @brief 处理取消请求
     *
     * 接受到取消信息时，打印取消信息并返回 ACCEPT，表示接受取消请求
     *
     * @param goal_handle
     * @return rclcpp_action::CancelResponse
     */
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    /**
     * @brief 当目标被接受时调用
     *
     * 由于ROS2的执行器不应阻塞，这里通过新建线程调用 execute 函数来执行目标的实际处理
     *
     * @param goal_handle
     */
    void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        using namespace std::placeholders;
        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
        std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
    }

    /**
     * @brief fibonacci计算的逻辑
     *
     * @param goal_handle
     */
    void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        rclcpp::Rate loop_rate(1);
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<Fibonacci::Feedback>();
        auto &sequence = feedback->partial_sequence;
        sequence.push_back(0);
        sequence.push_back(1);
        auto result = std::make_shared<Fibonacci::Result>();

        for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i)
        {
            // Check if there is a cancel request
            // 检查是否取消请求
            // 当为true时，表示客户端希望取消当前正在执行的目标
            if (goal_handle->is_canceling())
            {
                result->sequence = sequence;   // 设置结果序列，如果取消请求被确认，将当前的序列sequence赋值给result->sequence，目的是在取消后仍然可以返回已生成的序列
                goal_handle->canceled(result); // 调用取消方法，传递result作为参数，通知目标已经被取消，并且可以返回相关的结果
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            // Update sequence
            sequence.push_back(sequence[i] + sequence[i - 1]);
            // Publish feedback
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Publish feedback");

            loop_rate.sleep();
        }

        // Check if goal is done
        if (rclcpp::ok())
        {
            result->sequence = sequence;
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        }
    }
};